#include "PI.h"

PIController::PIController(float kp, float ki, float limit)
    : kp(kp), ki(ki), output_limit(limit), ui(0.0f), integral_clamped(false)
{
}

void PIController::reset() {
    ui = 0.0f;
    integral_clamped = false;
}

float PIController::update(float setpoint, float feedback) {
    float error = setpoint - feedback;
    
    // Calculate control output using ki and current integral state
    float u = ki * error + ui;
    
    // Apply output saturation
    if(u > output_limit) {
        u = output_limit;
        if(!integral_clamped) {
            ui = output_limit;
            integral_clamped = true;
        }
    } 
    else if(u < -output_limit) {
        u = -output_limit;
        if(!integral_clamped) {
            ui = -output_limit;
            integral_clamped = true;
        }
    }
    else {
        integral_clamped = false;
    }
    
    // Update integral state only if not clamped
    if(!integral_clamped) {
        ui += kp * error;
    }
    
    return u;
}
